#include "correspond.h"
#include <pcl/registration/correspondence_estimation.h>
#include <pcl/registration/correspondence_estimation_normal_shooting.h>
#include <pcl/registration/correspondence_rejection_sample_consensus.h>
#include <pcl/registration/correspondence_rejection_distance.h>
#include <pcl/registration/correspondence_rejection_median_distance.h>
#include <pcl/registration/correspondence_rejection_one_to_one.h>
#include <pcl/registration/correspondence_rejection_surface_normal.h>
#include <pcl/registration/correspondence_rejection_trimmed.h>
//#include <pcl/registration/correspondence_rejection_features.h>
//#include <pcl/registration/correspondence_rejection_var_trimmed.h>
#include <pcl/features/normal_3d.h>
#include "functions.h"

void findCorrespondencesFPFH (
        PointCloud<FPFHSignature33>::Ptr source,
        PointCloud<FPFHSignature33>::Ptr target,
        CorrespondencesPtr correspondences)
{
    //cout << "Finding correspondences..." << flush;

    registration::CorrespondenceEstimation<FPFHSignature33,FPFHSignature33> ce;
    ce.setInputCloud(source);
    ce.setInputTarget(target);
    ce.determineReciprocalCorrespondences(*correspondences);//evidentemente non va senza reciprocal
    //cout << "OK! Correspondences found: " << correspondences->size() << endl;
}




void filterCorrespondencesSAC ( PointCloud<PointXYZI>::Ptr source_keypoints,
                                PointCloud<PointXYZI>::Ptr target_keypoints,
                                CorrespondencesPtr correspondences,
                                double inlierThreshold)
{
    //cout << "Filtering correspondences SAC..." << flush;
    registration::CorrespondenceRejectorSampleConsensus<PointXYZI> rejector;
    rejector.setInputCloud(source_keypoints);
    rejector.setTargetCloud(target_keypoints);
    rejector.setMaxIterations(50);
    rejector.setInlierThreshold(inlierThreshold);
    rejector.setInputCorrespondences(correspondences);
    rejector.getCorrespondences(*correspondences);
    //cout << "OK! Correspondences filtered: " << correspondences->size()<< endl;
}

void addCorrespondencesToResult ( CorrespondencesPtr toAdd,
                                  CorrespondencesPtr result)
{
    for( int i=0;i<toAdd->size();i++)
        result->push_back(toAdd->at(i));
}

bool compare(Correspondence first,Correspondence second)
{
    return first.index_query<second.index_query;
}


//there are cloned correspondences. each correspondence has a distance value (between descriptors): only the min distance is kept
//query=object   match=cluster of world
void removeClones( CorrespondencesPtr input)
{
    sort(input->begin(),input->end(),compare);
    for (int i=input->size()-1;i>0;i--)
        if( input->at(i-1).index_query==input->at(i).index_query)
            if (input->at(i-1).distance < input->at(i).distance)
                input->erase(input->begin()+i); //i-1 is nearer
            else
                input->erase(input->begin()+i-1); // i is nearer


     //for (int i=input->size()-1;i>=0;i--)  cout <<"query "<< input->at(i).index_query<<" - match "<< input->at(i).index_match <<" - distance "<< input->at(i).distance  <<endl;
}
